#include "master.h"
#include "rtthread.h"
#include "rtdevice.h"

#include "srv_control.h"
#include "srv_sensor.h"
#include "task_fun.h"
#include "drv_motor.h"
#include "beep.h"
#include "pid.h"
#include "button.h"

#include "util_com.h"

PID_Data turn_pid;
static rt_sem_t two_driver_sem;
static uint8_t task_num=0;
static uint8_t finish_stop_flag = 0;


static void _main_driving(void *param)
{
    ist_ctr_set_normal_speed(0.0);

    ist_car_talk(COM_CMD_START);

    while (1)
    {
        if(finish_stop_flag)
        {
            ist_car_talk(COM_CMD_STOP);

            ist_motor_set(MOTOR_LEFT, 0.0);
            ist_motor_set(MOTOR_RIGHT, 0.0);

            ist_beep_on(1, 20000);
            return ;
        }

        rt_sem_take(two_driver_sem, RT_WAITING_FOREVER);
        rt_sem_release(two_driver_sem);

        double error;
        error = ist_sensor_get_direction_error();
        ist_pid_handle(&turn_pid, -error);
        ist_ctr_set_turn(turn_pid.output);

        rt_thread_mdelay(20);
    }
}

static void _task_1_driving(void *param)
{
    ist_ctr_set_normal_speed(TASK_1_SPEED);  //0.3m/s
    ist_fix_power_table(0); 
    int32_t current_time_origin= -999999;
    int32_t current_time_fork= -999999;
    while(1)
    {
        rt_sem_take(k210_rx_sem, RT_WAITING_FOREVER);
        rt_sem_take(two_driver_sem, RT_WAITING_FOREVER);

        switch (ist_get_road_signal_num())
        {
        case SIGNAL_FORK:
            if((rt_tick_get_millisecond()-current_time_fork)>100000)   //任务一只需要一次
            {
                rt_sem_release(two_driver_sem);
                current_time_fork = rt_tick_get_millisecond();

                ist_turn_low(1200, 1);
            }
            else
            {
                rt_sem_release(two_driver_sem);
            }
            break;

        case SIGNAL_ORIGIN:
            // finish_stop_flag = 1;
            // rt_kprintf("origin\n");
            if((rt_tick_get_millisecond()-current_time_origin)>100000)  //任务1只需要一次
            {
                rt_sem_release(two_driver_sem);
                current_time_origin = rt_tick_get_millisecond();

                ist_origin_signal_add();
                // rt_kprintf("origin");
                uint8_t count = ist_get_origin_signal_count();
                if(count >= 1)
                {
                    rt_thread_mdelay(1200);
                    finish_stop_flag = 1;
                }
            }
            else
            {
                rt_sem_release(two_driver_sem);                
            }
            break;  

        // case SIGNAL_STOP:
        //     // rt_kprintf("stop\n");
        //     rt_thread_mdelay(800);
        //     ist_signal_stop_5s();

        //     rt_sem_release(two_driver_sem);
        //     break;      
        default:
            rt_sem_release(two_driver_sem);
            break;
        }
        rt_thread_mdelay(10);
        ist_clean_road_signal_num();
        // ist_k210_re_finish();
    }
}

static void _task_2_driving(void *param)
{
    ist_ctr_set_normal_speed(TASK_2_SPEED);  //0.3m/s
    ist_fix_power_table(1); 
    int32_t current_time_origin= -999999;
    int32_t current_time_fork= -999999;
    while(1)
    {
        rt_sem_take(k210_rx_sem, RT_WAITING_FOREVER);
        rt_sem_take(two_driver_sem, RT_WAITING_FOREVER);
        // rt_kprintf("ok\n");
        // rt_kprintf("%d\n", ist_get_road_signal_num());
        switch (ist_get_road_signal_num())
        {
        case SIGNAL_FORK:
            if((rt_tick_get_millisecond()-current_time_fork)>8000)   
            {
                rt_sem_release(two_driver_sem);
                current_time_fork = rt_tick_get_millisecond();

                ist_turn_low(700, 2);
            }
            else
            {
                rt_sem_release(two_driver_sem);
            }
            break;

        case SIGNAL_ORIGIN:
            // finish_stop_flag = 1;
            // rt_kprintf("origin\n");
            if((rt_tick_get_millisecond()-current_time_origin)>3000)
            {
                rt_sem_release(two_driver_sem);
                current_time_origin = rt_tick_get_millisecond();

                ist_origin_signal_add();
                rt_kprintf("origin");
                uint8_t count = ist_get_origin_signal_count();
                if(count >= 2)
                {
                    rt_thread_mdelay(300);
                    finish_stop_flag = 1;
                }
            }
            else
            {
                rt_sem_release(two_driver_sem);                
            }
            break;  

        // case SIGNAL_STOP:
        //     // rt_kprintf("stop\n");
        //     rt_thread_mdelay(500);
        //     ist_signal_stop_5s();

        //     rt_sem_release(two_driver_sem);
        //     break;      
        default:
            rt_sem_release(two_driver_sem);
            break;
        }
        rt_thread_mdelay(10);
        ist_clean_road_signal_num();
        // ist_k210_re_finish();
    }
}

static void _task_3_driving(void *param)
{
    ist_ctr_set_normal_speed(TASK_3_SPEED);  //0.3m/s
    ist_fix_power_table(2); 
    int32_t current_time_origin= -999999;
    int32_t current_time_fork= -999999;
    int32_t current_time_stop= -999999;
    // int32_t current_time_stop= -999999;
    while(1)
    {
        rt_sem_take(k210_rx_sem, RT_WAITING_FOREVER);
        rt_sem_take(two_driver_sem, RT_WAITING_FOREVER);
        // rt_kprintf("ok\n");
        // rt_kprintf("%d\n", ist_get_road_signal_num());
        switch (ist_get_road_signal_num())
        {
        case SIGNAL_FORK:
            if((rt_tick_get_millisecond()-current_time_fork)>100000)   //任务一只需要一次
            {
                rt_sem_release(two_driver_sem);
                current_time_fork = rt_tick_get_millisecond();

                ist_turn_low(700, 1);
            }
            else
            {
                rt_sem_release(two_driver_sem);
            }
            break;

         case SIGNAL_ORIGIN:
            // finish_stop_flag = 1;
            // rt_kprintf("origin\n");
            if((rt_tick_get_millisecond()-current_time_origin)>100000)  //任务1只需要一次
            {
                rt_sem_release(two_driver_sem);
                current_time_origin = rt_tick_get_millisecond();

                ist_origin_signal_add();
                // rt_kprintf("origin");
                uint8_t count = ist_get_origin_signal_count();
                if(count >= 1)
                {
                    rt_thread_mdelay(300);
                    finish_stop_flag = 1;
                }
            }
            else
            {
                rt_sem_release(two_driver_sem);                
            }
            break;  

        case SIGNAL_STOP:
            // rt_kprintf("stop\n");
            if((rt_tick_get_millisecond()-current_time_stop)>100000)
            {
                current_time_stop = rt_tick_get_millisecond();

                rt_thread_mdelay(300);

                ist_car_talk(COM_CMD_STOP);

                ist_signal_stop_5s();
                ist_beep_on(1, 5000);

                // ist_signal_stop_5s();

                ist_car_talk(COM_CMD_START);

                rt_sem_release(two_driver_sem);

            }
            else
            {
                rt_sem_release(two_driver_sem);  
            }
            break;      
        default:
            rt_sem_release(two_driver_sem);
            break;
        }
        rt_thread_mdelay(10);
        ist_clean_road_signal_num();
        // ist_k210_re_finish();
    }
}



void ist_master_start(void)
{
    ist_pid_create(&turn_pid, TURN_KP, TURN_KI, TURN_KD);
    ist_pid_setmax(&turn_pid, 100);

    two_driver_sem = rt_sem_create("two_driver_sem", 1, RT_IPC_FLAG_FIFO);

    rt_thread_t thread = rt_thread_create("master", _main_driving, RT_NULL, 1024, 2, 10);
    rt_thread_startup(thread);

    switch(task_num)
    {
        case 1:
            thread = rt_thread_create("task_1", _task_1_driving, RT_NULL, 1024, 2, 10);
             rt_thread_startup(thread);
             break;
        case 2:
            thread = rt_thread_create("task_2", _task_2_driving, RT_NULL, 1024, 2, 10);
            rt_thread_startup(thread);
            break;
        case 3:
            thread = rt_thread_create("task_3", _task_3_driving, RT_NULL, 1024, 2, 10);
             rt_thread_startup(thread);
             break;
        default:
            break;
    }

    ist_beep_on(3, 50);
}

static void _get_task(void *param)
{
    while(1)
    {
        rt_sem_take(sw_2_sem, RT_WAITING_FOREVER);

        task_num++;
        if(task_num >3)
        {
            task_num = 0;
        }
        ist_beep_on(task_num, 150);

        rt_thread_mdelay(20);
    }
}

void ist_get_task_init(void)
{
    rt_thread_t thread = rt_thread_create("get_task", _get_task, RT_NULL, 1024, 2, 10);
    rt_thread_startup(thread);
}

// INIT_COMPONENT_EXPORT(_get_task_init);

